"""
http://www.multiwii.com/wiki/index.php?title=Multiwii_Serial_Protocol
From MultiWii 
Jump to: navigation, search

A discussion on the MSP is in this thread: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1516

The general format of an MSP message is:

<preamble>,<direction>,<size>,<command>,,<crc>

Where:

preamble = the ASCII characters '$M'

direction = the ASCII character '<' if to the MWC or '>' if from the MWC

size = number of data bytearray, binary. Can be zero as in the case of a data request to the MWC

command = message_id as per the table below

data = as per the table below. UINT16 values are LSB first.

crc = XOR of <size>, <command> and each data byte into a zero'ed sum 

--------------- Python Code Write By ShenZhen Joyton RD --- 2022-11-24
"""
import struct
import threading
import time
import serial
import signal
import os

### msp message ID
MSP_IDENT = 100
MSP_STATUS = 101
MSP_RAW_IMU = 102
MSP_SERVO = 103
MSP_MOTOR = 104
MSP_RC = 105
MSP_RAW_GPS = 106
MSP_COMP_GPS = 107
MSP_ATTITUDE = 108
MSP_ALTITUDE = 109
MSP_ANALOG = 110
MSP_RC_TUNING = 111
MSP_PID = 112
MSP_BOX = 113
MSP_MISC = 114
MSP_MOTOR_PINS = 115
MSP_BOXNAMES = 116
MSP_PIDNAMES = 117
MSP_WP = 118
MSP_BOXIDS = 119
MSP_SERVO_CONF = 120

MSP_SET_RAW_RC = 200
MSP_SET_RAW_GPS = 201
MSP_SET_PID = 202
MSP_SET_BOX = 203
MSP_SET_RC_TUNING = 204
MSP_ACC_CALIBRATION = 205
MSP_MAG_CALIBRATION = 206
MSP_SET_MISC = 207
MSP_RESET_CONF = 208
MSP_SET_WP = 209
MSP_SELECT_SETTING = 210
MSP_SET_HEAD = 211
MSP_SET_SERVO_CONF = 212
MSP_SET_MOTOR = 214
MSP_BIND = 240
MSP_EEPROM_WRITE = 250
### end msp message ID

###
MSP_DEBUG_ENABLE = False
def msp_debug(msg):
    if MSP_DEBUG_ENABLE: print(msg)

def msp_request_task_callback(msp_obj):
    while True:
        if msp_obj.fd!=None and msp_obj.fd.writable():
            now = time.time()*1000
            for k in msp_obj.request_list.keys():
                loop_time_ms = msp_obj.request_list[k][0]
                last_time_ms = msp_obj.request_list[k][1]
                if now-last_time_ms>loop_time_ms:
                    request_data = msp_obj.encode(msp_obj.TO_FC, k, ())
                    msp_obj.request_list[k][1] = now
                    msp_obj.fd.write(request_data)
        time.sleep(0.001)

def msp_parse_task_callback(msp_obj):
    count = 0
    while True:
        if msp_obj.fd!=None and msp_obj.fd.readable():
            data = []
            try:
                data = msp_obj.fd.read(1024)
            except: pass
            if len(data)>0:
                for d in data:
                    msg = msp_obj.decode(d)
                    if msg!=None:
                        msp_obj.update_time()
                        id = msg.msg_id
                        if id==MSP_ATTITUDE:
                            msp_obj.set_att(msp_attitude(msg))
                            msp_debug("->Attitude: (%.1f, %.1f, %.1f)" % (msp_obj.pitch, msp_obj.roll, msp_obj.yaw))
                            #pass
                        elif id==MSP_ANALOG:
                            msp_obj.set_vbat(msp_analog(msg))
                            count += 1
                            msp_debug("->Analog: %d -- %.2fV" % (count, msp_obj.vbat))
                            #pass
                        elif id==MSP_IDENT:
                            msp_obj.set_version(msp_ident(msg))
                            #msp_debug("->msp version: %d, type: %d" % (ver.get_msp_version, ver.get_multitype))
                            #pass
                        else: pass
        time.sleep(0.001)

class msp_message(object):
    def __init__(self, id, data):
        self.msg_id = id
        self.data_buff = data[:]
    
    def get_msg_id(self):
        return self.msg_id

class msp_ident():
    _MSG_ID = MSP_IDENT
    _format = '<BBBI'
    _size = 7
    version = 0
    multitype = 0
    msp_version = 0
    capability = 0

    def __init__(self, msg):
        self.msg_id = msg.msg_id
        self.data_buff = msg.data_buff[:]
        self.update()
        
    def update(self):
        if len(self.data_buff)==self._size:
            (self.version, self.multitype, self.msp_version, self.capability) = struct.unpack(self._format, bytearray(self.data_buff))
    
    def get_version(self):
        return self.version

    def get_multitype(self):
        return self.multitype

    def get_msp_version(self):
        return self.msp_version
    
    def get_capability(self):
        return self.capability

class msp_analog():
    _MSG_ID = MSP_ANALOG
    _format = '<BHHHH'
    _size = 9
    vbat = 0.0
    intPowerMeterSum = 0
    rssi = 0
    amperage = 0

    def __init__(self, msg):
        self.msg_id = msg.msg_id
        self.data_buff = msg.data_buff[:]
        self.update()

    def update(self):
        if len(self.data_buff)==self._size:
            (self.legacyBatteryVoltage, self.intPowerMeterSum, self.rssi, self.amperage, self.vbat) = struct.unpack(self._format, bytearray(self.data_buff))
            self.vbat = self.legacyBatteryVoltage*0.1
    
    def get_vbat(self):
        return self.vbat 
    
    def get_intPowerMeterSum(self):
        return self.intPowerMeterSum
    
    def get_rssi(self):
        return self.rssi

    def get_amperage(self):
        return self.amperage

### att
class msp_attitude():
    _MSG_ID = MSP_ATTITUDE
    _format = '<hhh'
    _size = 6
    _pitch = 0  #deg
    _roll = 0
    _yaw = 0

    def __init__(self, msg):
        self.msg_id = msg.msg_id
        self.data_buff = msg.data_buff[:]
        self.update()
        
    def update(self):
        if len(self.data_buff)==self._size:
            (self._roll, self._pitch, self._yaw) = struct.unpack(self._format, bytearray(self.data_buff))
            self._roll /= 10.0
            self._pitch /= 10.0

    def get_pitch(self):
        return self._pitch
    def get_roll(self):
        return self._roll
    def get_yaw(self):
        return self._yaw

### Multiwii Serial Protocol
class msp(object):
    _MSP_HEADER1 = ord('$')
    _MSP_HEADER2 = ord('M')
    TO_FC = ord('<')
    FROM_FC = ord('>')

    _msg_id = 0
    _msg_len = 0
    _msg_dir = 0
    _data_buff = []
    _data_index = 0
    _parse_flag = 0
    _parse_crc = 0
    _last_rec_time_ms = 0

    _MSP_PARSE_HEADER1 = 1
    _MSP_PARSE_HEADER2 = 2
    _MSP_PARSE_DIR = 3
    _MSP_PARSE_DATA_SIZE = 4
    _MSP_PARSE_MSG_ID = 5
    _MSP_PARSE_CRC = 6

    fd = None 
    rq_td = None
    parse_td = None

    request_list = {}  
    request_msg_thread = None  

    pitch = 0.0
    roll = 0.0
    yaw = 0.0
    vbat = 0.0
    version = 0
    msp_version = 0
    frame_type = 0

    def __init__(self, fd):
        self.fd = fd
        pass

    def set_att(self,  att):
        self.pitch = att.get_pitch()
        self.roll = att.get_roll()
        self.yaw = att.get_yaw()

    def set_vbat(self, ang):
        self.vbat = ang.get_vbat()
    
    def set_version(self, ver):
        self.msp_version = ver.get_msp_version()
        self.frame_type = ver.get_multitype()
        self.version = ver.get_version()

    def msp_comm_timeout(self):
        now = time.time()*1000
        return (now>self._last_rec_time_ms and now-self._last_rec_time_ms>=5000)

    def msp_start_process(self, obj):
        if self.rq_td==None:
            self.rq_td = threading.Thread(target=msp_request_task_callback, args=(obj,))
            self.rq_td.start()
        if self.parse_td==None:
            self.parse_td = threading.Thread(target=msp_parse_task_callback, args=(obj,))
            self.parse_td.start()

    def add_msg_request(self, id, req):
        if 0==req:
            if id in self.request_list.keys():
                del self.request_list[id]
            return
        ms = 1.0/req * 1000
        if ms<5: ms = 5
        self.request_list[id] = [ms, time.time()*1000]
            
    def encode(self, forward, id, data):
        rt = []
        if forward!=self.TO_FC and forward!=self.FROM_FC:
            return rt

        rt.append(self._MSP_HEADER1)
        rt.append(self._MSP_HEADER2)
        rt.append(forward)
        if data is None:
            rt.append(0)
        else:
            rt.append(len(data))
        rt.append(id)
        if data is not None:
            for d in data:
                rt.append(d)
        
        crc = 0
        for d in rt[3:]:
            crc = crc ^ d
        rt.append(crc)

        return rt
    
    def _reset_parse(self):
        self._msg_id = 0
        self._msg_len = 0
        self._data_index = 0
        self._parse_flag = 0
        self._parse_crc = 0
        self._data_buff = []

    def decode(self, data):
        data = ord(data)
        msg = None
        if self._MSP_HEADER1==data and 0==self._parse_flag:
            self._reset_parse()
            self._parse_flag = self._MSP_PARSE_HEADER1

        elif self._MSP_HEADER2==data and self._parse_flag==self._MSP_PARSE_HEADER1:
            self._parse_flag = self._MSP_PARSE_HEADER2
        
        elif self._parse_flag==self._MSP_PARSE_HEADER2:
            if data!=self.TO_FC and data!=self.FROM_FC:
                self._reset_parse()
            self._msg_dir = data
            self._parse_flag = self._MSP_PARSE_DIR
        
        elif self._parse_flag==self._MSP_PARSE_DIR:
            self._msg_len = data
            self._parse_flag = self._MSP_PARSE_DATA_SIZE
            self._parse_crc = self._parse_crc ^ data
        
        elif self._parse_flag==self._MSP_PARSE_DATA_SIZE:
            self._msg_id = data
            self._parse_flag = self._MSP_PARSE_MSG_ID
            if self._msg_len==0:
                self._parse_flag = self._MSP_PARSE_CRC
            self._parse_crc = self._parse_crc ^ data
        
        elif self._parse_flag==self._MSP_PARSE_MSG_ID:
            self._data_buff.append(data)
            self._msg_len = self._msg_len - 1
            if 0==self._msg_len:
                self._parse_flag = self._MSP_PARSE_CRC
            self._parse_crc = self._parse_crc ^ data
        
        elif self._parse_flag==self._MSP_PARSE_CRC:
            if self._parse_crc==data:
                self._parse_flag = 0
                if len(self._data_buff)>0:
                    msg = msp_message(self._msg_id, self._data_buff)
            else:
                self._parse_flag = 0
        else:
            self._reset_parse()

        return msg
    
    def update_time(self):
        self._last_rec_time_ms = time.time()*1000

    def get_msg_id(self):
        return self._msg_id

    def get_data(self):
        return self._data_buff

if __name__=="__main__":

    serial_fd = serial.Serial("/dev/ttyTHS1", 115200, timeout=0)

    msp_obj = msp(serial_fd)
    msp_obj.add_msg_request(MSP_ANALOG, 10)
    msp_obj.add_msg_request(MSP_ATTITUDE, 0.5)
    #msp_obj.add_msg_request(MSP_IDENT, 0.1)
    msp_debug(msp_obj.request_list)
    msp_obj.msp_start_process(msp_obj)
    msp_debug("msp start process...")

    try:
        while True:
            time.sleep(0.001)
    except KeyboardInterrupt:
        msp_obj.fd.close()
        os._exit(0)
